﻿/**
  通用的4点支架分为两种类型: Y型和X型
  */
#ifndef MEPTSHOLDER_H
#define MEPTSHOLDER_H

#include <memath/MePoint.h>

#include <Eigen/Core>

#include <iostream>
#include <vector>


/**
 * @brief 点的坐标和位置索引之间的关系
 *
 * i, j 属于 在输入的所有点中的索引
 * index 属于预先定义的距离的索引
 */
struct PointIndexPair {
    PointIndexPair(int _index, double _distance, int _i, int _j) {
        index = _index;
        distance = _distance;
        i = _i;
        j = _j;
    }

    static bool compPointIndexPair(const PointIndexPair &a, const PointIndexPair &b)
    {
        return a.index < b.index; //由小到大排序
    }

    int index;  // 预定义距离中的位置
    double distance;
    int i; // 第一个点的索引
    int j; // 第二个点的索引
};

/**
 * @brief 支架定义
 */
class MePtsHolder
{
public:
    enum HolderType {
        HolderTypeY= 0,
        HolderTypeX=1
    };

    MePtsHolder();
    /// 传入预先定义的6个距离
    MePtsHolder(double _dPrefdef[6]);
    virtual ~MePtsHolder() {}

    void setPredefDistance(double _dPredef[6]);

    /// 通过距离计算支架点的基础坐标
    virtual void calPtsByDisPredef(MePoint3D points[4]) {}

   /// 映射到对应的点,  输出为4个点
    /// outPt4pIndex 4点在 inPts 中的索引
    /// outPts 输出4个点，或者没有
    virtual int matchPoints(std::vector<MePoint3D> &inPts,  std::vector<int> &outPtsIndex, std::vector<MePoint3D> & outPts, const double eps=0.5);

    /// 校验距离的一致性， 6个距离，知道5个距离就确定了点的坐标， 6个距离多了
    //virtual bool verifyDisConsistency() = 0;

    /// 从正视图4个点计算对应的点
    /// 输入4个点, 输出4个点
    virtual bool calPtsFromFrontView(std::vector<MePoint3D> &inPts, std::vector<MePoint3D> &outPts) {return true;}
    /// 获取需要重新计算的点，默认p4
    virtual int getOpPt() { return 3;}
    virtual void calHolderTransformWith3Pt(std::vector<MePoint3D> &baseHolderPts, std::vector<MePoint3D> &holderYPts,
                Eigen::Matrix3d &R_YtoB, Eigen::Vector3d &T_YtoB);
    /// 根据距离，将输入点的坐标按照预定顺序输出，输入可能有杂点
    /// outPt4pIndex 4点在 inPs 中的索引
    static int calPt4PFromPefDefDis(double prefDefDis[6], double inPs[], int inPsSize,
            int outPt4pIndex[4], double outPt4P[3*4], const double eps = 1.0);
    /// 根据距离，将输入点的坐标按照预定顺序输出，输入可能有杂点
    /// outPt4pIndex 4点在 inPts 中的索引
    /// outPts 输出4个点，或者没有
    static int calPt4PFromPefDefDis(double prefDefDis[6], std::vector<MePoint3D> &inPts,
            int outPt4pIndex[4], MePoint3D outPts[4], const double eps = 1.0);
    static int calPt4PFromPefDefDis(std::vector<double> prefDefDis, std::vector<MePoint3D> &inPts,
            int outPt4pIndex[4], MePoint3D outPts[4], const double eps = 1.0);

//    /// 转成QString
//    static const QString ptsToQString(double MePoint3D pts[4]) {
//        return QString::asprintf("%8.4f, %8.4f, %8.4f\n%8.4f, %8.4f, %8.4f\n%8.4f, %8.4f, %8.4f\n%8.4f, %8.4f, %8.4f",
//                inPts[0], inPts[1], inPts[2], inPts[3], inPts[4], inPts[5],
//                inPts[6], inPts[7], inPts[8], inPts[9], inPts[10], inPts[11]
//                );
//    }

    /// 计算输入4个点(p1, p2, p3, p4)的距离
    /// 距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    static void calPt4PDistance(double inP[12], double outDis[6]);
    static void calPt4PDistance(double p1[3],double p2[3], double p3[3], double p4[3], double outDis[6]);
    static void calPt4PDistance(MePoint3D pts[4], double outDis[6]);

    // 在误差允许范围内
    static inline bool isInError(double value, const double eps = 1.0)
    {
        if (value >eps || value < -eps) {
            return false;
        } else {
            return true;
        }
    }

    /// 从预先定义的距离里面，找到对应的位置。-1 表示错误
    static int getPredefDisIndex(double pefDefDis[6], double dis);
    /// 是否和预定的尺寸相等
    static bool isPt4PWithPredefDis(double prefDefDis[6], double p1[3],double p2[3], double p3[3], double p4[3]);
    /// 是否和预定的尺寸相等
    static bool isPt4PWithPredefDis(double prefDefDis[6], MePoint3D points[4]);

private:
    static bool findCorrectPt4PIndex(double prefDefDis[6], double inPs[], std::vector<PointIndexPair> vectorPair, int pt4pIndex[4]);
    static bool findCorrectPt4PIndex(double prefDefDis[6], std::vector<MePoint3D> &inPts, std::vector<PointIndexPair> vectorPair, int pt4pIndex[4]);


public:
    std::string name = "undef";
    /// 6个点的距离(预先定义好的)。距离的顺序为：{p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    std::vector<double> disPredef;
    /// 4个基础点的坐标，具体参见具体的实现类
    std::vector<MePoint3D> pts;

    MePoint3D fingerPt; // 指头点
    std::vector<MePoint3D> ptsRelated;  // 支架上相关联的点
    HolderType holderType = HolderTypeY;
};


/// Y型支架, 具体图见 product_merobot\Y型支架图.png
/// disPredef 预先知道的6个距离，输出4个点
/// 设定z=0， Y支架，右边高左边点， P2点为Y中间点， P1为Y下端点，P2P1为X正方向，
/// z轴从纸面出来，左边支点P3， 右边支点P4
class MePtsHolderTypeY : public MePtsHolder
{
public:
    MePtsHolderTypeY();
    MePtsHolderTypeY(double _dPrefdef[6]);

    /// 通过距离计算实际的初始点的坐标，原始平面z=0
    /// 按照p3为原点计算, p3p1 为x轴, 计算当前坐标系
    /// 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    virtual void calPtsByDisPredef(MePoint3D points[4]) override;
    virtual void calPtsByDisPredef(MePoint3D points[4], double p3ToPlane);
    /// 按照p3为原点计算, p3p1 为x轴, 计算当前坐标系
    virtual void calPtsByDisPredefOp3(MePoint3D points[4]);

    /// 映射到对应的点,  输出为4个点
    /// outPt4pIndex 4点在 inPts 中的索引
    /// outPts 输出4个点，或者没有
    // int matchPoints(std::vector<MePoint3D> &inPts, int outPtsIndex[4], std::vector<MePoint3D> & outPts, const double eps=0.5);

    /// 获取需要重新计算的点，p2
    virtual int getOpPt() override{ return 1;}
    /// 通过三点计算旋转矩阵
    virtual void calHolderTransformWith3Pt(std::vector<MePoint3D> &baseHolderPts, std::vector<MePoint3D> &holderYPts,
                Eigen::Matrix3d &R_YtoB, Eigen::Vector3d &T_YtoB) override;

    /// 从正视图4个点计算对应的点
    /// 输入4个点, 输出4个点
    virtual bool calPtsFromFrontView(std::vector<MePoint3D> &inPts, std::vector<MePoint3D> &outPts) override;
};

/// X型支架， 一般定义左下角点为原点(P1), 逆时针依次为P2, P3, P4
/// P1P2为X轴，垂直向上为Y轴，纸面穿出为Z轴
class MePtsHolderTypeX : public MePtsHolder
{
public:
    MePtsHolderTypeX();

    /// 通过距离计算实际的初始点的坐标，原始平面z=0
    /// 按照p1为原点计算, p1p2 为x轴, 计算当前坐标系
    /// 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    virtual void calPtsByDisPredef(MePoint3D points[4]) override;

    /// 从正视图4个点计算对应的点
    /// 输入4个点, 输出4个点
    virtual bool calPtsFromFrontView(std::vector<MePoint3D> &inPts, std::vector<MePoint3D> &outPts) override;

};

/// TFM102支架定义
class MePtsHolderTFM102
{
public:
    MePtsHolderTFM102();

    /// 映射到对应的点
    int matchPoints(std::vector<MePoint3D> &inPts, int outPtsIndex[5], std::vector<MePoint3D> & outPts, const double eps=0.5);

public:
    /// 7个点的距离(预先定义好的)。距离的顺序为：{p0p2, p0p3, p0p4, p2p3, p2p4, p3p4, p0p1}
    double disPredef[7];
    std::vector<MePoint3D> ptsPredef;
};

#endif // MEPTSHOLDER_H
